# DIT IS EEN WORK IN PROGRESS! ALLEEN EDITEN OP TOESTEMMING VAN AUKE/CAMIEL
# TODo Minor bug fixes. hij sluit niet af na FINISHED en je kunt hem niet op nieuw starten zonder uit te loggen en de code op nieuw te laden+runne
# Edited by Sander @ 15.13 28-3-2011 -> added buttonlistener
# Edited by Erik @ 30-3-2011 --> buttonlistener commented out, gc = gamec.GameController() added. Other changes see StartSoul()

#############################################################
# soul.py : the nao's footballplaying soul.					#
# states call phases										#
# phases call moves											#
# Transitions between states through global string state	#
# Transitions between phases through global string phase	#
# state : e.g. "Playing"									#
# phase : e.g. "BallFound"									#
#############################################################

from naoqi import ALProxy
import Queue
import motionInterface_IR as mot
import visionInterface_IR as vis 
import GameController_V8_IR as gamec
import Buttons2_IR as buttons
import threading
import time
import math

# Initialization of the Variables
global state
global phase
global ball_loc
global ball_loc2
global teamColor
global mean
global kickOff
global playerType

global robot
global team
f = open('naoinfo/NAOINFO', 'r')
robot = int(f.readline())-1#this reads the first line
team = int(f.readline())#this reads the second line

ball_loc = Queue.Queue(4)
ball_loc2 = Queue.Queue(4)
state = 0
phase = "BallNotFound"
teamColor = None
mean = ((0,0), (0,0))

# PROXIES
balldetect = ALProxy("ALRedBallDetection", "localhost", 9559)
balltrack = ALProxy("ALRedBallTracker", "localhost", 9559)
leds = ALProxy("ALLeds", "localhost", 9559)
video = ALProxy("ALVideoDevice", "localhost", 9559)
video.setParam(18,1)

balltrack.setWholeBodyOn(False)

buttonListener = buttons.buttonListener()
gc = gamec.GameController(team)

class stateController(threading.Thread):
	def __init__(self, threadID, name):
		threading.Thread.__init__(self)
		self.threadID = threadID
		self.name = name

	def run(self):
		self.getState()

	def getState(self):
		global state
		global gcActive
                gcActive = True
		buttonListener = buttons.buttonListener()
                lastCheck = 0
		while(state != 4 ):
			gcActive = gc.controlledRefresh()
			lastCheck = time.time()
			if gcActive:
				if gc.getPenalty(robot, team) !=0:
                                        state = 10
                                else:
                                        state = gc.getGameState() #get phase from GameController
			else:
				while time.time() - lastCheck < 30:
					buttonListener.refresh()
					if buttonListener.getChestButton(): #change the phase if the ChestButton was pressed
						if(state == 0):
							state = 10
						elif(state == 10):
							state = 3
						elif(state == 3):
							state = 10
						else:
							state = 3
sc = stateController(1, "stateController")

def printState():
        print "robot: ", robot
	print "team: ", team
	time.sleep(1)
	while 1:
		print state
                print "gcActive", gcActive

###############################################################
# FUNCTIONS USED
# Motion
# normalPose()
# post.walkto()
# scanCircle()
# standup()
# rKickAngled()
# lKickAngled()
# hakje()
# FootRight()
# FootLeft()
# DiveRight()
# DiveLeft()
# stance()
###############################################################

#####################
# stateS			#
# Initial()			#
# Ready()			#
# Set()				#
# Playing()			#
# Penalized()		#
# Finished()		#
#####################

# Initial state: LEDs are off
def Initial(): # NormalPose
	global teamColor
	global kickOff
	global playerType
	leds.off("AllLeds")
	mot.stiff()
	mot.move("normalHead")
	mot.move("normalPose")

	playerType = 0
        print gc.controlledRefresh()		
	if gc.controlledRefresh():
			teamColor = gc.getTeamColor(team)
			kickOff = gc.getKickoff()
	else:
			if buttonListener.getSetup():
					(teamColor, kickOff) = buttonListener.setUp(0, 0)
	if playerType == 1:
		print "Keeper"
	else:
		print "Player"
	
# Ready state: 
def Ready():
	leds.fadeRGB("ChestLeds",0x000000ff, 0)
	if (color == 0):
		leds.fadeRGB("RightFootLeds", 0x000000ff, 0)
		leds.fadeRGB("LeftFootLeds", 0x000000ff, 0)
	else:
		leds.fadeRGB("RightFootLeds", 0x00ff0000, 0)
		leds.fadeRGB("LeftFootLeds", 0x00ff0000, 0)
	
		

def Set():
	global phase	
	leds.fadeRGB("ChestLeds",0x00ffff00, 0)
	# head movements allowed in the set state
	if not(balltrack.isActive()):
		balltrack.startTracker()
		video.setParam(18,1)
		balltrack.getPosition()
	if not(balltrack.isNewData()):
		if mot.scanCircle(2.0):
			if playerType == 0:
				phase = "BallFound"
			else:
				phase = "BallFoundKeep"
		else:
			if playerType == 0:
				phase = "BallNotFound"
			else:
				phase = "BallNotFoundKeep"
		print phase

def Playing():
	global phase
	leds.fadeRGB("ChestLeds", 0x0000ff00, 0)
	if mot.standUp():
		if playerType == 0:
			phase = "BallNotFound"
		else:
			phase = "BallFoundKeep"
	phases.get(phase)()
	print phase

def Penalized():
	global phase
	leds.fadeRGB("ChestLeds",0x00ff0000, 0)
	if balltrack.isActive():
		balltrack.stopTracker()
		if playerType == 0:
			phase = "BallNotFound"
		else:
			phase = "BallNotFoundKeep"	
	mot.killAllTasks()
	
def Finished():
	leds.off("AllLeds")
	balltrack.stopTracker()
	mot.killAllTasks()
	vis.off()
	gc.socket.close()
	
#########################
# phases Player			#
# BallFound()			#
# BallNotFound()		#
#########################
# phases Keeper			#
# BallFoundKeep()		#
# BallNotFoundKeep()	#
# BallApproaching()		#
# InGoalArea()			#
#########################

#############
# Keeper	#
#############

def BallFoundKeep():
	global phase
	global ball_loc
	global ball_loc2
	global mean
	if balltrack.isNewData():
		(x0,y0,z) = balltrack.getPosition()
		x0 = evaluatex(x0)
		y0 = evaluatey(y0)

		(ball_loc_mean, ball_loc2_mean) = mean
		(xnew, ynew) = ball_loc_mean	
		(xold, yold) = ball_loc2_mean
			
		if ball_loc.empty():
			mot.stance()
		
		# remove if too full 
		if ball_loc.full():
			(x4,y4) = ball_loc.get()
			xnew = (4.0 * xnew - x4 ) / 3.0
			ynew = (4.0 * ynew - y4 ) / 3.0
			
			# remove if too full 2
			if ball_loc2.full():
				(x8,y8) = ball_loc2.get()
				xold = (4.0 * xold - x8 ) / 3.0
				yold = (4.0 * yold - y8 ) / 3.0

			# update mean 2
			ball_loc2_len = ball_loc2.qsize()
			xold = (ball_loc2_len * xold + x4 ) / (ball_loc2_len+1)
			yold = (ball_loc2_len * yold + y4 ) / (ball_loc2_len+1)
			ball_loc2.put((x4,y4))

		# update mean			
		ball_loc_len = ball_loc.qsize()			
		xnew = (ball_loc_len * xnew + x0 ) / (ball_loc_len+1)
		ynew = (ball_loc_len * ynew + y0 ) / (ball_loc_len+1)
		ball_loc.put((x0,y0))
		
		ball_loc_mean = (xnew, ynew) 
		ball_loc2_mean = (xold, yold)
		mean = (ball_loc_mean, ball_loc2_mean) 
		print mean
		
		# only after a certain number of measurements
		if ball_loc2.full():
			# calc diff in distance
			distold = math.sqrt(xold**2 + yold**2)
			distnew = math.sqrt(xnew**2 + ynew**2)
			speed = distold - distnew
			# calculate direction
			if speed > 0.2:
				print speed
				dir = (((yold - ynew )* xnew) / (xold - xnew)) - ynew
				while not(ball_loc.empty()):
					ball_loc.get()
				ball_loc.put(dir)
				phase = "BallApproaching"
				print dir
				return True
			elif(xnew < 0.5 and (ynew < 1.1 or ynew > -1.1) and speed <= 0.2):
				phase = "InGoalArea"
				print speed
		
		# counter
		start = time.time()
		while time.time() - start < 0.4 and not(balltrack.isNewData()):
			pass
		
	else:
		phase = "BallNotFoundKeep"

def BallNotFoundKeep():
	global phase
	global ball_loc
	global ball_loc2
	global mean
	if not(balltrack.isActive()):
		balltrack.startTracker()
		video.setParam(18,1)
	if not(ball_loc2.empty()):
		while not(ball_loc.empty()):
			ball_loc.get()
		while not(ball_loc2.empty()):
			ball_loc2.get()
		mean = ((0,0),(0,0))
		mot.normalPose()
	if mot.scanCircle(1):
		phase = "BallFoundKeep" 
		
def BallApproaching():
	global phase
	global ball_loc
	dir = ball_loc.get()
	balltrack.stopTracker()
	if dir >= 0.4:
		mot.diveRight()
	elif dir <= -0.4:
		mot.diveLeft()
	elif dir < 0.4 and dir >= 0:
		mot.footRight()
	elif dir > -0.4 and dir < 0:
		mot.footLeft()
	balltrack.startTracker()
	video.setParam(18,1)
	phase = "BallNotFoundKeep"

def InGoalArea():
	global phase
	global mean
	if not(mot.isWalking()):
		(x,y,z) = balltrack.getPosition()
		y = evaluatey(y)
		x = evaluatex(x)
		if x > 0.1 and math.fabs(y) > 0.05:
			if y > 0:
				mot.move("postWalkTo", (x-0.075,y-0.04,0))
			else:
				mot.move("postWalkTo", (x-0.075,y+0.04,0))
		else:
			(currentx, currenty, z) = balltrack.getPosition()
			balltrack.stopTracker()
			mot.move("normalPose")
			(ball_loc_mean, ball_loc2_mean) = mean
			(x, y) = ball_loc_mean
			if ynew < 0:
				mot.move("rKickAngled", 0)
			else:
				mot.move("lKickAngled", 0)
			mot.move("walkTo", (-x/3.0 + currentx, -y/3.0 + currenty, 0 ))
			mot.move("walkTo", (-x / 3.0, y / 3.0, 0 ))
			mot.move("walkTo", (-x / 3.0, y / 3.0, 0 ))
			balltrack.startTracker()
			video.setParam(18,1)
	phase = "BallNotFoundKeep"
			
#############
# Player	#
#############

def BallFound():
	global phase
	if balltrack.isNewData():
		(x,y,z) = balltrack.getPosition()
		x = evaluatex(x)
		y = evaluatey(y)
		walkangle = angle(x,y)
		print x,y
		if x > 0.1 and x < 0.3 and y > -0.04 and y < 0.04:
			print 'kick'
			mot.move("postWalkTo", (x-0.08, y, 0))
			balltrack.stopTracker()
			mot.move("normalPose")
			mot.move("normalHead")
			phase = "Kick"
			
		else:
			mot.move("postWalkTo", (x,y/2.0,walkangle/2.0))
			start = time.time()
			while not(balltrack.isNewData()) and time.time() - start < 0.4:
				pass
	else:
		phase = "BallNotFound"
		while not(ball_loc.empty()):
			ball_loc.get()
			
def BallNotFound():
	global phase
	global ball_loc
	if not(balltrack.isActive()):
		balltrack.startTracker()
		video.setParam(18,1)
	if ball_loc.empty():
		mot.move("postWalkTo", (0,0,0.0001))
	if mot.scanCircle(1.1):
		phase = "BallFound"
	else:
		mot.move("normalHead")
		angles = vis.findBall()
		if angles:
			(x,y) = angles
			mot.move("walkTo", (0, 0, x))
			mot.move("postWalkTo", (0.8 - 2*y, 0, 0))
			while not(ball_loc.empty()):
				ball_loc.get()
			ball_loc.put(1)
		else:
			mot.move("postWalkTo", (0, 0, 1.5))
			while not(ball_loc.empty()):
				ball_loc.get()
			ball_loc.put(1)
	
def Kick():
	global phase
	print 'getgoal'
	headangle = 0
	goal = vis.findGoal()
	print goal
	if not(goal):
		mot.move("setHead", (1, -0.5))
		goal = vis.findGoal()
		headangle = 1
		print goal
	if not(goal):
		mot.move("setHead", (-1, -0.5))
		goal = vis.findGoal()
		headangle = -1
		print goal
	if not(goal):
		print 'no goal'
		headangle = 0.9
		goal = (2, 0)
		print goal
	(color, kickangle) = goal
	if color == "Blue":
		teamColor = 0
	else:
		teamColor = 1
	# if you see your own goal, kick to the other side
	if color == teamColor:
		if headangle == 1:
			kickangle = -2
		if headangle == -1:
			kickangle = 2
		else:
			mot.move("walkTo", (0, 0.04, 0))
			mot.move("hakje")
			phase = "BallNotFound"
			return True
	kickangle = headangle + kickangle
	if kickangle > 1:
		kickangle = 1.05
	if kickangle < -1:
		kickangle = -1.05
	mot.move("setHead", (0,0))
	convert = 1.047 # (60 degrees rad)
	print kickangle/convert
	if kickangle <= 0:
		mot.move("walkTo", (0, -0.04, 0))
		mot.move("normalPose")
		mot.move("lKickAngled", kickangle/ -convert)
	elif kickangle > 0:
		mot.move("walkTo", (0, 0.04, 0))		
		mot.move("normalPose")
		mot.move("rKickAngled", kickangle/ convert)
	phase = "BallNotFound"
	balltrack.startTracker()
	video.setParam(18,1)	

#############
# EXTRA     #
#############

def angle(x,y):
	m = math.atan(y/x)
	if x >= 0:
		return m
	return -math.pi + m
	
def evaluatex(x):
	return x * 1.58 - 0.64
	
def evaluatey(y):
	pitch = mot.getHead()[1]
	return y / (2.5 - (0.5 - pitch)/10.0)
	
#############
# OS		#
#############
def startSoul():
	global state
        sc.start()
        mot.stiff()

	while(state != 4):
		states.get(state)()
	states.get(state)() # Voor finished state*

# states
states = 	{
	0: Initial,
	1: Ready,
	2: Set,
	3: Playing,
	4: Finished,
	10: Penalized
	}

# phases
phases = 	{
	"BallFound": BallFound,
	"BallNotFound": BallNotFound,
	"Kick" : Kick,
	"BallFoundKeep": BallFoundKeep,
	"BallNotFoundKeep": BallNotFoundKeep,
	"BallApproaching": BallApproaching,
	"InGoalArea": InGoalArea
}
